/*
Copyright 2012 Cuong Pham

This file is part of eTrack project.

eTrack is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by the
Free Software Foundation, either version 3 of the License, or (at your
option) any later version.

eTrack is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.

You should have received a copy of the GNU General Public License
along with eTrack. If not, see http://www.gnu.org/licenses/.
*/

/***********************************************************************************************
 *    Project Name: eTrack
 *    Project website: http://code.google.com/p/etrack/
 * *********************************************************************************************/

import java.awt.*;
import java.applet.*;
import java.awt.event.*;
import java.io.BufferedReader;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintStream;
import java.util.Random;
import com.rapplogic.xbee.api.XBee;
import com.rapplogic.xbee.api.XBeeException;
import com.rapplogic.xbee.api.XBeeResponse;

class Localizer implements Runnable
{    localizationApplet g;    
    
    public Localizer(localizationApplet gs) { g = gs; }
    
    public synchronized void run()
    {    
        try{
            g.dataFeed();
        }
        catch(Exception e){}
    }
}

class stopLocalizer implements Runnable
{    localizationApplet g;    
    
    public stopLocalizer(localizationApplet gs) { g = gs; }
    
    public synchronized void run()
    {    
        try{
            g.stop();
        }
        catch(Exception e){}
    }
}


public class localizationApplet extends Applet
{
    private static int RUN_NUMBER=1;
    public static String rootTraining = "C:\\TrainingData\\Run"+RUN_NUMBER+"\\";    
    private static Graphics g;
    private Thread ActionThread = null;
    private Thread ActionThread1 = null;
    int [][]count;

    public static int RESET = 100;
    public static int RSSI = 102;
    private static int NUM_ANCHORS=16;
    private static int NUM_CLASSES=10;
    private static int NUM_CLASSES_X=9;
    private static int NUM_CLASSES_Y=10;

    
    private int NUM_COLS=9;
    private int NUM_ROWS=10;
    private int FIELD_WIDTH=369;
    private int FIELD_HEIGHT=400;
    private int CELL_WIDTH=FIELD_WIDTH/NUM_CLASSES_X;
    private int CELL_HEIGHT=FIELD_HEIGHT/NUM_CLASSES_Y;
    private int FIELD_STARTX=100;
    private int FIELD_STARTY=100;

    private static boolean stop=false;
    
    static String modelsFolder = "C:\\TrainingData\\Run"+RUN_NUMBER+"\\Models\\";
    static String libsvmFolder = "C:\\libsvm\\windows\\";
    static String fTest="test.txt";
    static String fPredict="test.txt.predict";
    
    //Scale
    static Scale scaleX, scaleY;
    //Model
    static Model[] modelX, modelY;

    Font bigFont;
    Font smallFont;

    Color blackColor;
    Color redColor;
    Color weirdColor;
    Color bgColor;

    //Buttons
    Button startButton;
    Button stopButton; 
     
     public void init() 
     {
         System.out.println("init");
         count =new int[NUM_COLS][NUM_ROWS];
         bigFont = new Font("Arial",Font.BOLD,20);
         smallFont = new Font("Arial",Font.PLAIN,12);

         startButton = new Button("Start");
         stopButton = new Button("Stop"); 
       
         add(startButton);
         add(stopButton); 
          
          // Attach actions to the components
          startButton.addActionListener(new startListener());
          stopButton.addActionListener(new stopListener());

          g=getGraphics();
          
          //get scale
          scaleX=new Scale(rootTraining+"Models\\classX1.txt.range");
          scaleY=new Scale(rootTraining+"Models\\classY1.txt.range");
          
          //get Models
          modelX=new Model[NUM_CLASSES_X];
          modelY=new Model[NUM_CLASSES_Y];
          for (int i=0;i<NUM_CLASSES_X;i++){
              String fn=rootTraining+"Models\\classX"+(i+1)+".txt.model";
              modelX[i]= new Model(fn);
          }
          
          System.out.println("Done modelX!");

          for (int i=0;i<NUM_CLASSES_Y;i++){
              String fn=rootTraining+"Models\\classY"+(i+1)+".txt.model";
              modelY[i]= new Model(fn);
          }
          System.out.println("Done modelY!");
     }

    public void stop()
     {
         stop=true;
         
         for (int i=0; i<NUM_COLS;i++)
             for (int j=0;j<NUM_ROWS;j++)
                 count[i][j]=0;
             
         repaint();
     }

     // now lets draw things on screen
     public void paint(Graphics g) 
     {
         startButton.setBounds(FIELD_STARTX+FIELD_WIDTH-100, 50, 50, 25);
         stopButton.setBounds(FIELD_STARTX+FIELD_WIDTH-50, 50, 50, 25);
         g.setFont(bigFont);
         g.drawString("Sensors Map",100,60);

         g.setFont(smallFont);
         g.drawString("0",90,95);
         
         g.setColor(blackColor);

          g.drawRect(100,100,FIELD_WIDTH,FIELD_HEIGHT);

          float dash1[] = {5.0f};
          BasicStroke dashed =
                new BasicStroke(1.0f,
                                BasicStroke.CAP_BUTT,
                                BasicStroke.JOIN_MITER,
                                10.0f, dash1, 0.0f);
          ((Graphics2D) g).setStroke(dashed);
      
      int x1,y1,x2,y2;
      x1=x2=FIELD_STARTX;
      y1=FIELD_STARTY;
      y2=FIELD_STARTY+FIELD_HEIGHT;
      int rangeX=FIELD_WIDTH/NUM_COLS;
      for (int i=1;i<=NUM_COLS;i++){
          x1+=rangeX;
          x2=x1;
          g.drawString(""+i,x1-3,95);
          g.drawLine(x1,y1,x2,y2);
      }
      x1=x2=FIELD_STARTX;
      x1=FIELD_STARTX;
      x2=FIELD_STARTX+FIELD_WIDTH;
      int rangeY=FIELD_HEIGHT/NUM_ROWS;
      for (int i=1;i<=NUM_ROWS;i++){
          y1+=rangeY;
          y2=y1;
          g.drawString(""+i,80,y1+3);
          g.drawLine(x1,y1,x2,y2);
      }
     }

    public void plot(int x, int y, int radius, Color c){
         // x,y = grid coordinate e.g., (0, 0), (4, 5), ...
         
         int param=2;
         int x0=(2 * FIELD_STARTX * NUM_CLASSES_X +(2*x+1)*FIELD_WIDTH)/(2*NUM_CLASSES_X);
         int y0=(2 * FIELD_STARTY * NUM_CLASSES_Y +(2*y+1)*FIELD_HEIGHT)/(2*NUM_CLASSES_Y);

         g.setColor(c);
         g.fillRect(x0-radius+1, y0-radius+1, param*radius-2, param*radius-2);
     }
     
     
     public void dataFeed()throws IOException, XBeeException{
         int newPosX=0;
         int newPosY=0;
         int oldPosX=0;
         int oldPosY=0;
            
         int newPosXX=0;
         int newPosYY=0;
         int oldPosXX=0;
         int oldPosYY=0;

         stop=false;
         XBee xbee = new XBee();
         int []arrX=new int[NUM_CLASSES_X];
         int []arrY=new int[NUM_CLASSES_Y];
         for (int i=0;i<NUM_CLASSES_X;i++){
           arrX[i]=i+1;
           arrY[i]=i+1;
         }
         arrY[NUM_CLASSES_Y-1]=NUM_CLASSES_Y;


         try {
      // replace with your com port and baud rate. this is the com port of my coordinator
           xbee.open("COM11", 9600);

         int source = 0;
         int msgType = 0;
         int serial = 0;
         int rssi = 0;
         int[] data=new int[NUM_ANCHORS+1];
         int MAX_COUNT=10;
         int count1=0;
         int max_count = 0;
         int oldPosX_est = 0;
         int oldPosY_est = 0;
         
         for (int i=0;i<NUM_ANCHORS+1;i++) data[i]=0;

         while (!stop) {
           XBeeResponse response = xbee.getResponse();
           source =
   response.getProcessedPacketBytes()[response.getLength().get16BitValue()-4];
           msgType =
   response.getProcessedPacketBytes()[response.getLength().get16BitValue()-1];
           serial =
   response.getProcessedPacketBytes()[response.getLength().get16BitValue()];
           rssi =
   response.getProcessedPacketBytes()[response.getLength().get16BitValue()+1];

           if (msgType !=RSSI) continue;

           data[source]=rssi;
           // now, all anchors have received RSSI
           
           double[] dataScaleX=new double[NUM_ANCHORS];
           double[] dataScaleY=new double[NUM_ANCHORS];

           
           //do prediction

           for (int i=1;i<NUM_ANCHORS+1;i++){
             System.out.print(i+":"+data[i]+" ");
           }
           System.out.println();

           for (int k=0;k<NUM_ANCHORS;k++){
             dataScaleX[k]=scaleX.scaleAttr(k+1, data[k+1]);
             dataScaleY[k]=scaleY.scaleAttr(k+1, data[k+1]);
           }

           //do localization
           newPosX=localize("X",arrX,dataScaleX);
           newPosY=localize("Y",arrY,dataScaleY);
           System.out.println("x="+newPosX+" y="+newPosY);
 
           plot(oldPosX_est,oldPosY_est,10,Color.white);
           oldPosX_est=(newPosX+oldPosX)/2;
           oldPosY_est=(newPosY+oldPosY)/2;
           plot(oldPosX_est,oldPosY_est,10,Color.red);
           oldPosX=newPosX; oldPosY=newPosY;
           
             
         } // end of while loop
        }
         catch (Exception e){}
         finally {
            xbee.close();
         }
       }
     
    // precondition: array a[] is sorted
     public int localize(String type, int[] a, double[] b) {
         int lo = 0;
         int hi = a.length-1;
         while (true) {
             // Key is in a[lo..hi] or not present.

             if (hi - lo == 1) return a[lo];
             int mid = (lo + hi) / 2;
             //System.out.println("lo="+(lo+1)+" mid="+(mid+1)+"hi="+(hi+1));
             //System.out.flush();
             double label =0;
             if (type=="X"){
                 label= modelX[a[mid]].H(b);
                 
             }
             else if (type=="Y"){
                 label= modelY[a[mid]].H(b);
                 
             }
                     
             
             if (label > 0) hi = mid;
             else lo = mid;
             
         }
     }

     
    static int check(String type, int loc){
        
        String scale="";
        String command="";
        scale+=libsvmFolder+"svm-scale -r "+rootTraining+"Models\\class"+type+loc+".txt.range "+fTest;
        command+=libsvmFolder+"svm-predict "+fTest+".scale "+" "+modelsFolder+"class"+type+loc+".txt.model "+fPredict;
        //System.out.println(scale);
        try{
            Process p1 = Runtime.getRuntime().exec(scale);
            BufferedReader in = new BufferedReader(
            new InputStreamReader(p1.getInputStream()) );
            FileOutputStream fOutStream = new FileOutputStream(fTest+".scale");
            DataOutputStream out = new DataOutputStream(fOutStream);

            out.writeBytes(in.readLine()+"\n");
            in.close();
            out.close();
            Thread.sleep(100);
            Process p = Runtime.getRuntime().exec(command);
        }
        catch (Exception e) {
            System.out.println("exception happened - here's what I know: ");
            e.printStackTrace();
            System.exit(-1);
        }
        
        try{
            Thread.sleep(100);
            FileInputStream fstream = new FileInputStream(fPredict);
            DataInputStream in = new DataInputStream(fstream);
            BufferedReader br = new BufferedReader(new InputStreamReader(in));
            String strLine;
            if ((strLine = br.readLine()) != null){
                return Integer.parseInt(strLine);
            }
            else return 0;


        }
        
         catch (Exception e){//Catch exception if any
          System.err.println("Error: " + e.getMessage());
         }

        return 0;        
    }

    double distance(int[]a,int[] b){
        
        long sum=0;
        for (int i=0;i<b.length;i++){
            sum+=Math.pow(a[i+1]-b[i],2);
        }
        return Math.sqrt(sum);
    }
    
    void getScale(Scale s){
        String fScale=rootTraining+"Models\\classX1.txt.range";
        try{
            FileInputStream fstream = new FileInputStream(fScale);
            DataInputStream in = new DataInputStream(fstream);
            BufferedReader br = new BufferedReader(new InputStreamReader(in));
            String strLine;
            strLine=br.readLine();
            strLine=br.readLine();
            
            s.lo=Double.parseDouble(strLine.split(" ")[0]);
            s.hi=Double.parseDouble(strLine.split(" ")[1]);
            
            for (int i=0;i<NUM_ANCHORS;i++){
                strLine=br.readLine();
                s.attrRange[i][0]=Double.parseDouble(strLine.split(" ")[1]);
                s.attrRange[i][1]=Double.parseDouble(strLine.split(" ")[2]);
            }
        
        }
        catch(Exception e){
            System.err.println("Error: " + e.getMessage());
        }
    }

    class startListener implements ActionListener
    {
        public void actionPerformed(ActionEvent e)
        {    ActionThread = new Thread(new Localizer(localizationApplet.this));
            ActionThread.start();
        }
    }

    class stopListener implements ActionListener
    {
        public void actionPerformed(ActionEvent e)
        {    ActionThread1 = new Thread(new stopLocalizer(localizationApplet.this));
            ActionThread1.start();
        }
    }

    static class Model{
        public double gamma;
        public double rho;
        public int total_sv;
        public double[][] matrix;
        
        public Model(String fModel){
            //String fModel=rootTraining+"Models\\classX1.txt.model";
            try{
                FileInputStream fstream = new FileInputStream(fModel);
                DataInputStream in = new DataInputStream(fstream);
                BufferedReader br = new BufferedReader(new InputStreamReader(in));
                String strLine;
                strLine=br.readLine();
                strLine=br.readLine();
                strLine=br.readLine();
                
                gamma=Double.parseDouble(strLine.split(" ")[1]);
                
                strLine=br.readLine();
                strLine=br.readLine();
                total_sv=Integer.parseInt(strLine.split(" ")[1]);
                matrix=new double[total_sv][NUM_ANCHORS+1];
                
                strLine=br.readLine();
                rho=Double.parseDouble(strLine.split(" ")[1]);

                strLine=br.readLine();
                strLine=br.readLine();
                strLine=br.readLine();

                for (int i=0;i<total_sv;i++){
                    //System.out.print(i+" ");
                    strLine=br.readLine();
                    //System.out.print(strLine);
                    String[] vals=strLine.split(" ");
                    //System.out.print(vals.length);
                    //System.out.print(vals[1]);
                    matrix[i][0]=Double.parseDouble(vals[0]);
                    //System.out.print(matrix[i][0]+" ");
                    for (int j=1;j<vals.length;j++){
                        matrix[i][Integer.parseInt(vals[j].split(":")[0])]=Double.parseDouble(vals[j].split(":")[1]);
                    //    System.out.print(matrix[i][j]+" ");
                    }
                    //System.out.println();
                }
                in.close();
            }catch(Exception e){}
            
            
        }
        public void print(){
            System.out.println("gamma="+gamma+"rho="+rho+"total_sv="+total_sv);
            for (int i=0;i<total_sv;i++){
                for (int j=0;j<=NUM_ANCHORS;j++){
                    System.out.print(matrix[i][j]+" ");
                }
                System.out.println();
            }
            
        }

        double K(double[] a, double[] b){
            double distance=0;
            for (int i=0;i<a.length;i++){
                distance+=Math.pow(a[i]-b[i],2);
            }
            
            return Math.exp((0-gamma)*distance);
            
        }
        
        public double H(double[] a){
            double sum=0;
            for (int i=0;i<total_sv;i++){
                double[] b=new double[NUM_ANCHORS];
                for (int j=1;j<=NUM_ANCHORS;j++){
                    b[j-1]=matrix[i][j];
                }
                sum+=matrix[i][0]*K(a,b);
            }
            return sum-rho;
        }


    }
    
    class Scale{
        public double lo,hi;
        public double [][] attrRange;
        
        public Scale(String fScale){
            lo=hi=0;
            attrRange=new double[NUM_ANCHORS][2];
            try{
                FileInputStream fstream = new FileInputStream(fScale);
                DataInputStream in = new DataInputStream(fstream);
                BufferedReader br = new BufferedReader(new InputStreamReader(in));
                String strLine;
                strLine=br.readLine();
                strLine=br.readLine();
                
                lo=Double.parseDouble(strLine.split(" ")[0]);
                hi=Double.parseDouble(strLine.split(" ")[1]);
                
                for (int i=0;i<NUM_ANCHORS;i++){
                    strLine=br.readLine();
                    attrRange[i][0]=Double.parseDouble(strLine.split(" ")[1]);
                    attrRange[i][1]=Double.parseDouble(strLine.split(" ")[2]);
                }
            
            }
            catch(Exception e){
                System.err.println("Error: " + e.getMessage());
            }

        }
        
        public void print(){
            System.out.println("lo="+lo+" hi="+hi);
            for (int i=0;i<NUM_ANCHORS;i++){
                System.out.println((i+1)+" "+attrRange[i][0]+" "+attrRange[i][1]);
            }
        }
        
        public double scaleAttr(int attr, double val){
            return ((val-attrRange[attr-1][0])/(attrRange[attr-1][1]-attrRange[attr-1][0]))*(hi-lo)+lo;
            
        }
    }
    

} 
